
import math
import maya.OpenMaya as om
import AntsCrowd.FCVector as FCVector

reload( FCVector )

class matrix2D:
    _11=0.0
    _12=0.0
    _13=0.0
    _21=0.0
    _22=0.0
    _23=0.0
    _31=0.0
    _32=0.0
    _33=0.0
    def __init__(self):
        self.identity()
    
    def identity(self):
        self._11=1.0
        self._12=0.0
        self._13=0.0
        self._21=0.0
        self._22=1.0
        self._23=0.0
        self._31=0.0
        self._32=0.0
        self._33=1.0
        
    def matrixmul(self,mat):
        temp = matrix2D()
        temp._11=(self._11*mat._11) + (self._12*mat._21) + (self._13*mat._31)
        temp._12=(self._11*mat._12) + (self._12*mat._22) + (self._13*mat._32)
        temp._13=(self._11*mat._13) + (self._12*mat._23) + (self._13*mat._33)
        temp._21=(self._21*mat._11) + (self._22*mat._21) + (self._23*mat._31)
        temp._22=(self._21*mat._12) + (self._22*mat._22) + (self._23*mat._32)
        temp._23=(self._21*mat._13) + (self._22*mat._23) + (self._23*mat._33)
        temp._31=(self._31*mat._11) + (self._32*mat._21) + (self._33*mat._31)
        temp._32=(self._31*mat._12) + (self._32*mat._22) + (self._33*mat._32)
        temp._33=(self._31*mat._13) + (self._32*mat._23) + (self._33*mat._33)
        
        self._11 = temp._11
        self._12 = temp._12
        self._13 = temp._13
        self._21 = temp._21
        self._22 = temp._22
        self._23 = temp._23
        self._31 = temp._31
        self._32 = temp._32
        self._33 = temp._33
        
    def tranlate(self,x,y):
        mat = matrix2D()
        mat._11=1.0
        mat._12=0.0
        mat._13=0.0
        mat._21=0.0
        mat._22=1.0
        mat._23=0.0
        mat._31=x
        mat._32=y
        mat._33=1
        self.matrixmul(mat)
        
    def scale(self,x,y):
        mat = matrix2D()
        mat._11=x
        mat._12=0.0
        mat._13=0.0
        mat._21=0.0
        mat._22=y
        mat._23=0.0
        mat._31=0.0
        mat._32=0.0
        mat._33=1
        self.matrixmul(mat)
        
    def rotateex(self,fwd,side):
        mat = matrix2D()
        mat._11=fwd.x
        mat._12=fwd.y
        mat._13=0.0
        mat._21=side.x
        mat._22=side.y
        mat._23=0.0
        mat._31=0.0
        mat._32=0.0
        mat._33=1
        self.matrixmul(mat)
        
    def rotate(self,rot):
        mat = matrix2D
        Sin = math.sin(rot)
        Cos = math.cos(rot)
        mat._11=Cos
        mat._12=Sin
        mat._13=0.0
        mat._21=-Sin
        mat._22=Cos
        mat._23=0.0
        mat._31=0.0
        mat._32=0.0
        mat._33=1
        self.matrixmul(mat)
                
    
    def transformVector2D( self, vpoint ):
        tempX = (self._11 * vpoint.x) + (self._21 * vpoint.y) + (self._31)
        tempY = (self._12 * vpoint.x) + (self._22 * vpoint.y) + (self._32)
        return FCVector.vector2D( tempX, tempY )
        
#List to Matrix    
def getMMatrix(inputList):
    matrix = om.MMatrix() 
    om.MScriptUtil.createMatrixFromList(inputList, matrix)
    return matrix
#Matrix to List    
def setMMatrix(inputMatrix):
    returnList = []
    for row in range(0,4):
       for col in range(0,4):
    	   matrixValue = 0.0
    	   matrixValue = om.MScriptUtil.getDoubleArrayItem(inputMatrix[row] , col)
           returnList.append( matrixValue )
    return returnList
#get Matrix to Translate      
def getMtoT(matlist):
    _rotateM = getMMatrix(matlist)      
    matlist = setMMatrix(_rotateM)               
    return matlist[12],matlist[13],matlist[14]
    
def getMtoTV(matlist, _translate ):
    _rotateM = getMMatrix(matlist)
    matlist = setMMatrix(_rotateM)
    _translate.x = matlist[12]
    _translate.y = matlist[13]
    _translate.z = matlist[14]
    
#get Matrix to Rotate
def getMtoR(matlist):
    _rotateM = getMMatrix(matlist)      
    matlist = setMMatrix(_rotateM)               
    if matlist[4]>0.998:
        _heading = math.atan2(matlist[2],matlist[10])
        _attitude = PI/2
        _bank = 0            
        return _attitude,_heading,_bank
    if matlist[4]<-0.998:
        _heading = math.atan2(matlist[3],matlist[10])
        _attitude = PI/2
        _bank = 0            
        return _attitude,_heading,_bank
    #Y rotate
    _heading = math.atan2(-matlist[8],matlist[0])
    #z rotate
    _bank = math.atan2(-matlist[6],matlist[5])
    #x rotate
    _attitude = math.asin(matlist[4])
    return _attitude,_heading,_bank    